#include "niming.h"
#include "JY901B_931.h"
#include "data_define.h"
#include "niming_flow.h"
#include "sins.h"
#include "PPM.h"
#include <rtthread.h>
#include <rtdevice.h>
#include "delay_uil.h"
//#include "flow_pro.h"
#define HWTIMER_DEV_NAME   "timer13"     /* 定时器名称 */
u8 data2swji[100];
nimingshow_data NM_data={0};

#define SAMPLE_UART_NAME       "uart4"      /* 串口设备名称 */
/* 用于接收消息的信号量 */
static rt_device_t serial;
static struct serial_configure config2 = RT_SERIAL_CONFIG_DEFAULT;  /* 初始化配置参数 */
static uint8_t _datatemp[50];
void niming_task(void *p)
{
  u8  send_sta = 0;
  u32 t_now,t_preiod,t_pre ;
  void *delay;
  delay = delay_until_create("delay");  // 创建相对延时
  rt_thread_mdelay(500);
  flight_stat(2,1,2,3);
  send_sta = 1;
  while (1)
  {
      if(send_sta == 1)
      {
          USART1_niming_report_F1(JY901_data.angel.y*100,JY901_data.angel.x*100//1 2
                                  ,JY901_data.angel.z*100 ,JY901_data.height// 3 4
                                  ,sins.pos.z*100 ,control_data.get_vel.x// 5 6
                                  ,control_data.get_vel.y ,control_data.get_position.x// 7 8
                                  ,control_data.get_position.y , control_data.get_vel.z*100);//9 10
          USART1_niming_report_F2(control_data.height_set*100,control_data.basethrust*100,PPM_con.PPM_thrust*100
                  ,control_data.set_angel.x*100
                  ,control_data.set_angel.y*100
                  ,control_data.set_position.z*100,control_data.get_position.z*100
                  ,control_data.get_vel.z*100,control_data.height*100,110.235*100);

          USART1_niming_report_angel(control_data.get_angel.y*100,control_data.get_angel.x*100,control_data.get_angel.z*100,0);
          USART1_niming_report_dis( control_data.get_position.z*100, control_data.get_position.z*100,1);
          USART1_niming_report_speed(control_data.get_vel.x*100,control_data.get_vel.y*100,control_data.get_vel.z*100);
          USART1_niming_report_position(control_data.get_position.x*100,control_data.get_position.y*100);
          USART1_niming_report_batterVol(control_data.batter_vol*100,0);
          USART1_niming_report_GPS(10,5,(s32)stcLonLat.lLon,(s32)stcLonLat.lLat,stcGPSV.sGPSHeight,JY901_data.GPSV,JY901_data.GPSV,JY901_data.GPSV
                  ,100,100,100);
          USART1_niming_report_RC(0,0,PPM_con.PPM_thrust,0,channel_1,channel_2,channel_3,channel_4,channel_5,channel_6);
      }
      //**********任务运行时间统计**********
      t_now = rt_tick_get();//要赋值给整型数据
      t_preiod=t_now-t_pre;
      task_time1.niming_time = 1000/t_preiod;
      t_pre = t_now;
      rt_thread_mdelay(10);
//      delay_until_run(delay, 5);
  }
}

static void AnoOF_DataAnl2(uint8_t *data, uint8_t len)
{
    u8 check_sum1 = 0, check_sum2 = 0;
    if (*(data + 3) != (len - 6)) //ÅÐ¶ÏÊý¾Ý³¤¶ÈÊÇ·ñÕýÈ·
        return;
    for (u8 i = 0; i < len - 2; i++)
    {
        check_sum1 += *(data + i);
        check_sum2 += check_sum1;
    }
    if ((check_sum1 != *(data + len - 2)) || (check_sum2 != *(data + len - 1))) //ÅÐ¶ÏsumÐ£Ñé
        return;
    //================================================================================

    if (*(data + 2) == 0XE0) //¹âÁ÷ÐÅÏ¢
    {
        if (*(data + 4) == 0x10) //Ô­Ê¼¹âÁ÷ÐÅÏ¢
        {
            if (*(data + 5) == 0x02){
                if (*(data + 6) == 0x03){

                    NM_data.hor_move_dis = *((s16 *)(data + 7));
                    NM_data.hor_move_vel = *((s16 *)(data + 9));
                    NM_data.hor_move_angel = *((s16 *)(data + 11));
                }
                else if(*(data + 6) == 0x07) {
                    NM_data.turnL_angel = *((s16 *)(data + 7));
                    NM_data.turnL_rad = *((s16 *)(data + 9));
                }
                else if(*(data + 6) == 0x08) {
                    NM_data.turnR_angel = *((s16 *)(data + 7));
                    NM_data.turnR_rad = *((s16 *)(data + 9));
                }
                else if(*(data + 6) == 0x01) {
                    NM_data.up_dis = *((s16 *)(data + 7));
                }
                else if(*(data + 6) == 0x02) {
                    NM_data.down_dis = *((s16 *)(data + 7));
                }
            }
            else if(*(data + 5) == 0x00){
                if(*(data + 6) == 0x05){

                }
            }


        }
    }
}
static void AnoOF_GetOneByte2(uint8_t data)
{
    static u8 _data_len = 0, _data_cnt = 0;
    static u8 rxstate = 0;

    if (rxstate == 0 && data == 0xAA)
    {
        rxstate = 1;
        _datatemp[0] = data;
    }
    else if (rxstate == 1 && (data == HW_TYPE || data == HW_ALL))  //0x61  0xFF
    {
        rxstate = 2;
        _datatemp[1] = data;
    }
    else if (rxstate == 2 )
    {
        rxstate = 3;
        _datatemp[2] = data;
    }
    else if (rxstate == 3 && data < 250)
    {
        rxstate = 4;
        _datatemp[3] = data;
        _data_len = data;
        _data_cnt = 0;
    }
    else if (rxstate == 4 && _data_len > 0)
    {
        _data_len--;
        _datatemp[4 + _data_cnt++] = data;
        if (_data_len == 0)
            rxstate = 5;
    }
    else if (rxstate == 5)
    {
        rxstate = 6;
        _datatemp[4 + _data_cnt++] = data;
    }
    else if (rxstate == 6)
    {
        rxstate = 0;
        _datatemp[4 + _data_cnt] = data;
        //      DT_data_cnt = _data_cnt+5;
        //
        AnoOF_DataAnl2(_datatemp, _data_cnt + 5); //
    }
    else
    {
        rxstate = 0;
    }
}
/* 接收数据回调函数 */
static rt_err_t uart_input2(rt_device_t dev, rt_size_t size)
{
    char ucData;
    rt_device_read(serial, -1, &ucData, 1);
//    uart_GetOneByte(ucData);
    AnoOF_GetOneByte2(ucData);
//      Flow_Get_for_cheap(ucData);
//    K210Prose(ucData);
}
void niming_UART_init(void )
{
    /* 查找串口设备 */
    serial = rt_device_find(SAMPLE_UART_NAME);
    if (!serial)
    {
        rt_kprintf("find %s failed!\n", SAMPLE_UART_NAME);
    }
    else rt_kprintf("find %s is ok!\n", SAMPLE_UART_NAME);
    /* step2：修改串口配置参数 */
//    config2.baud_rate = BAUD_RATE_19200;        //修改波特率为
    config2.baud_rate = BAUD_RATE_115200;        //修改波特率为
    config2.data_bits = DATA_BITS_8;           //数据位 8
    config2.stop_bits = STOP_BITS_1;           //停止位 1
    config2.bufsz     = 128;                   //修改缓冲区 buff size 为 128
    config2.parity    = PARITY_NONE;           //无奇偶校验位
    /* step3：控制串口设备。通过控制接口传入命令控制字，与控制参数 */
    rt_device_control(serial, RT_DEVICE_CTRL_CONFIG, &config2);
    /* step4：打开串口设备。以中断接收及轮询发送模式打开串口设备 */
    rt_device_open(serial, RT_DEVICE_FLAG_INT_RX);
    /* 以读写及中断接收方式打开串口设备 */
    rt_device_open(serial, RT_DEVICE_OFLAG_RDWR | RT_DEVICE_FLAG_INT_RX);
    /* 设置接收回调函数 */
    rt_device_set_rx_indicate(serial, uart_input2);
   /* 发送字符串 */
//    rt_device_write(serial, 0, str, (sizeof(str) - 1));
    rt_thread_mdelay(10);
    rt_kprintf("flow_init %s is ok! \n",SAMPLE_UART_NAME);
}

void niming_task_init(void)
{
    rt_thread_t tid1 = RT_NULL;
    niming_UART_init();
    tid1 = rt_thread_create("niming_task",niming_task, RT_NULL, 2000,niming_task_prio, 20);
    if (tid1 != RT_NULL)rt_thread_startup(tid1),rt_kprintf("...niming_task is ok!\n");
}
INIT_APP_EXPORT(niming_task_init);
void UART_send_char(u8 c)
{
    while((UART4->SR&0X40)==0);
    UART4->DR=c;
}

void USART1_niming_report_HeightControl1(s16 ax,s16 ay,s16 az,s16 gx,s16 gy,s16 gz,u8 st)
{
  u8 _cnt=0;
    u8 sc = 0;
    u8 ac = 0;
    u8 i;
    data2swji[_cnt++] = 0xAA;
    data2swji[_cnt++] = 0xFF;
    data2swji[_cnt++] = 0x01;
    data2swji[_cnt++] = 13;

    data2swji[_cnt++] = BYTE0(ax);
    data2swji[_cnt++] = BYTE1(ax);

    data2swji[_cnt++] = BYTE0(ay);
    data2swji[_cnt++] = BYTE1(ay);

    data2swji[_cnt++] = BYTE0(az);
    data2swji[_cnt++] = BYTE1(az);

    data2swji[_cnt++] = BYTE0(gx);
    data2swji[_cnt++] = BYTE1(gx);

    data2swji[_cnt++] = BYTE0(gy);
    data2swji[_cnt++] = BYTE1(gy);

    data2swji[_cnt++] = BYTE0(gz);
    data2swji[_cnt++] = BYTE1(gz);

    data2swji[_cnt++] = st;

    for( i=0;i<data2swji[3]+4; i++)
    {
        sc += data2swji[i];
        ac += sc;
    }

    data2swji[_cnt++] = sc;
    data2swji[_cnt++] = ac;

    for(i=0;i<_cnt;i++)UART_send_char(data2swji[i]);
}
void USART1_niming_report_speed(u16 a,u16 b,u16 c)//ÉÏ´«µçÑ¹a£¬µçÁ÷b
{
    u8 _cnt=0;
    u8 sc = 0;
    u8 ac = 0;
    u8 i;
    data2swji[_cnt++] = 0xAA;
    data2swji[_cnt++] = 0xFF;
    data2swji[_cnt++] = 0x07;
    data2swji[_cnt++] = 6;

    data2swji[_cnt++] = BYTE0(a);
    data2swji[_cnt++] = BYTE1(a);

    data2swji[_cnt++] = BYTE0(b);
    data2swji[_cnt++] = BYTE1(b);

    data2swji[_cnt++] = BYTE0(c);
    data2swji[_cnt++] = BYTE1(c);

    for( i=0;i<data2swji[3]+4; i++)
    {
        sc += data2swji[i];
        ac += sc;
    }

    data2swji[_cnt++] = sc;
    data2swji[_cnt++] = ac;

    for(i=0;i<_cnt;i++)UART_send_char(data2swji[i]);
    //printf("%s\n",data2swji);

}
void USART1_niming_report_position(s32 a,s32 b)//ÉÏ´«µçÑ¹a£¬µçÁ÷b
{
    u8 _cnt=0;
    u8 sc = 0;
    u8 ac = 0;
    u8 i;
    data2swji[_cnt++] = 0xAA;
    data2swji[_cnt++] = 0xFF;
    data2swji[_cnt++] = 0x08;
    data2swji[_cnt++] = 8;

    data2swji[_cnt++] = BYTE0(a);
    data2swji[_cnt++] = BYTE1(a);
    data2swji[_cnt++] = BYTE2(a);
    data2swji[_cnt++] = BYTE3(a);

    data2swji[_cnt++] = BYTE0(b);
    data2swji[_cnt++] = BYTE1(b);
    data2swji[_cnt++] = BYTE2(b);
    data2swji[_cnt++] = BYTE3(b);

    for( i=0;i<data2swji[3]+4; i++)
    {
        sc += data2swji[i];
        ac += sc;
    }

    data2swji[_cnt++] = sc;
    data2swji[_cnt++] = ac;

    for(i=0;i<_cnt;i++)UART_send_char(data2swji[i]);
    //printf("%s\n",data2swji);

}
void USART1_niming_report_dis(s32 a,s32 b,u8 fu)//ÉÏ´«¸ß¶È
{
  u8 _cnt=0;
    u8 sc = 0;
    u8 ac = 0;
    u8 i;
    data2swji[_cnt++] = 0xAA;
    data2swji[_cnt++] = 0xFF;
    data2swji[_cnt++] = 0x05;
    data2swji[_cnt++] = 9;

    data2swji[_cnt++] = BYTE0(a);
    data2swji[_cnt++] = BYTE1(a);
    data2swji[_cnt++] = BYTE2(a);
    data2swji[_cnt++] = BYTE3(a);

    data2swji[_cnt++] = BYTE0(b);
    data2swji[_cnt++] = BYTE1(b);
    data2swji[_cnt++] = BYTE2(b);
    data2swji[_cnt++] = BYTE3(b);

    data2swji[_cnt++] = fu;


    for( i=0;i<data2swji[3]+4; i++)
    {
        sc += data2swji[i];
        ac += sc;
    }

    data2swji[_cnt++] = sc;
    data2swji[_cnt++] = ac;

    for(i=0;i<_cnt;i++)UART_send_char(data2swji[i]);
    //printf("%s\n",data2swji);

}
void USART1_niming_report_angel(s16 a,s16 b,s16 c,u8 fu)//ÉÏ´«roll£¬pitch£¬yaw
{
  u8 _cnt=0;
    u8 sc = 0;
    u8 ac = 0;
    u8 i;
    data2swji[_cnt++] = 0xAA;
    data2swji[_cnt++] = 0xFF;
    data2swji[_cnt++] = 0x03;
    data2swji[_cnt++] = 7;

    data2swji[_cnt++] = BYTE0(a);
    data2swji[_cnt++] = BYTE1(a);

    data2swji[_cnt++] = BYTE0(b);
    data2swji[_cnt++] = BYTE1(b);

    data2swji[_cnt++] = BYTE0(c);
    data2swji[_cnt++] = BYTE1(c);

    data2swji[_cnt++] = fu;


    for( i=0;i<data2swji[3]+4; i++)
    {
        sc += data2swji[i];
        ac += sc;
    }

    data2swji[_cnt++] = sc;
    data2swji[_cnt++] = ac;

    for(i=0;i<_cnt;i++)UART_send_char(data2swji[i]);
    //printf("%s\n",data2swji);

}
void USART1_niming_report_batterVol(s16 a,s16 b)//ÉÏ´«µçÑ¹a£¬µçÁ÷b
{
    u8 _cnt=0;
    u8 sc = 0;
    u8 ac = 0;
    u8 i;
    data2swji[_cnt++] = 0xAA;
    data2swji[_cnt++] = 0xFF;
    data2swji[_cnt++] = 0x0D;
    data2swji[_cnt++] = 4;

    data2swji[_cnt++] = BYTE0(a);
    data2swji[_cnt++] = BYTE1(a);

    data2swji[_cnt++] = BYTE0(b);
    data2swji[_cnt++] = BYTE1(b);

    for( i=0;i<data2swji[3]+4; i++)
    {
        sc += data2swji[i];
        ac += sc;
    }

    data2swji[_cnt++] = sc;
    data2swji[_cnt++] = ac;

    for(i=0;i<_cnt;i++)UART_send_char(data2swji[i]);
    //printf("%s\n",data2swji);

}
void flight_stat(u8 a,u8 b ,u8 c,u16 d)
{
  u8 _cnt=0 ,sc = 0,ac = 0,i;
    data2swji[_cnt++] = 0xAA;
    data2swji[_cnt++] = 0xFF;
    data2swji[_cnt++] = 0x06;
    data2swji[_cnt++] = 5;

    data2swji[_cnt++] = a;

    data2swji[_cnt++] = b;

    data2swji[_cnt++] = c;

    data2swji[_cnt++] = BYTE0(d);
    data2swji[_cnt++] = BYTE1(d);

    for( i=0;i<data2swji[3]+4; i++)
    {
        sc += data2swji[i];
        ac += sc;
    }

    data2swji[_cnt++] = sc;
    data2swji[_cnt++] = ac;

    for(i=0;i<_cnt;i++)UART_send_char(data2swji[i]);
}
void USART1_niming_report_HeightControl2(s16 a,s16 b,s16 c,s32 d , s16 e ,u8 f ,u8 g)//
{
    u8 _cnt=0;
    u8 sc = 0;
    u8 ac = 0;
    u8 i;
    data2swji[_cnt++] = 0xAA;
    data2swji[_cnt++] = 0xFF;
    data2swji[_cnt++] = 0x02;
    data2swji[_cnt++] = 14;

    data2swji[_cnt++] = BYTE0(a);
    data2swji[_cnt++] = BYTE1(a);

    data2swji[_cnt++] = BYTE0(b);
    data2swji[_cnt++] = BYTE1(b);

    data2swji[_cnt++] = BYTE0(c);
    data2swji[_cnt++] = BYTE1(c);

    data2swji[_cnt++] = BYTE0(d);
    data2swji[_cnt++] = BYTE1(d);
    data2swji[_cnt++] = BYTE2(d);
    data2swji[_cnt++] = BYTE3(d);

    data2swji[_cnt++] = BYTE0(e);
    data2swji[_cnt++] = BYTE1(e);

    data2swji[_cnt++] = f;

    data2swji[_cnt++] = g;


    for( i=0;i<data2swji[3]+4; i++)
    {
        sc += data2swji[i];
        ac += sc;
    }

    data2swji[_cnt++] = sc;
    data2swji[_cnt++] = ac;

    for(i=0;i<_cnt;i++)UART_send_char(data2swji[i]);
    //printf("%s\n",data2swji);

}
void USART1_niming_report_F1(s32 a,s16 b,s16 c,s16 d,s16 e,s16 vx,s16 vy,s16 dx,s16 dy,s16 vz)//ÉÏ´«roll£¬pitch£¬yaw
{
  u8 _cnt=0;
    u8 sc = 0;
    u8 ac = 0;
    u8 i;
    data2swji[_cnt++] = 0xAA;
    data2swji[_cnt++] = 0xFF;
    data2swji[_cnt++] = 0xF1;
    data2swji[_cnt++] = 20;

    data2swji[_cnt++] = BYTE0(a);
    data2swji[_cnt++] = BYTE1(a);

    data2swji[_cnt++] = BYTE0(b);
    data2swji[_cnt++] = BYTE1(b);

    data2swji[_cnt++] = BYTE0(c);
    data2swji[_cnt++] = BYTE1(c);

    data2swji[_cnt++] = BYTE0(d);
    data2swji[_cnt++] = BYTE1(d);

    data2swji[_cnt++] = BYTE0(e);
    data2swji[_cnt++] = BYTE1(e);

    data2swji[_cnt++] = BYTE0(vx);
    data2swji[_cnt++] = BYTE1(vx);

    data2swji[_cnt++] = BYTE0(vy);
    data2swji[_cnt++] = BYTE1(vy);

    data2swji[_cnt++] = BYTE0(dx);
    data2swji[_cnt++] = BYTE1(dx);

    data2swji[_cnt++] = BYTE0(dy);
    data2swji[_cnt++] = BYTE1(dy);

    data2swji[_cnt++] = BYTE0(vz);
    data2swji[_cnt++] = BYTE1(vz);
//    data2swji[_cnt++] = fu;


    for( i=0;i<data2swji[3]+4; i++)
    {
        sc += data2swji[i];
        ac += sc;
    }

    data2swji[_cnt++] = sc;
    data2swji[_cnt++] = ac;

    for(i=0;i<_cnt;i++)UART_send_char(data2swji[i]);
    //printf("%s\n",data2swji);

}
void USART1_niming_report_F2(s32 d1,s32 d2,s32 d3,s32 d4,s32 d5 ,s32 d6,s32 d7,s32 d8,s32 d9,s32 d10)
{
    u8 _cnt=0;
    u8 sc = 0;
    u8 ac = 0;
    u8 i;

    data2swji[_cnt++] = 0xAA;
    data2swji[_cnt++] = 0xFF;
    data2swji[_cnt++] = 0xF2;
    data2swji[_cnt++] = 40;

    data2swji[_cnt++] = BYTE0(d1);
    data2swji[_cnt++] = BYTE1(d1);
    data2swji[_cnt++] = BYTE2(d1);
    data2swji[_cnt++] = BYTE3(d1);

    data2swji[_cnt++] = BYTE0(d2);
    data2swji[_cnt++] = BYTE1(d2);
    data2swji[_cnt++] = BYTE2(d2);
    data2swji[_cnt++] = BYTE3(d2);

    data2swji[_cnt++] = BYTE0(d3);
    data2swji[_cnt++] = BYTE1(d3);
    data2swji[_cnt++] = BYTE2(d3);
    data2swji[_cnt++] = BYTE3(d3);

    data2swji[_cnt++] = BYTE0(d4);
    data2swji[_cnt++] = BYTE1(d4);
    data2swji[_cnt++] = BYTE2(d4);
    data2swji[_cnt++] = BYTE3(d4);

    data2swji[_cnt++] = BYTE0(d5);
    data2swji[_cnt++] = BYTE1(d5);
    data2swji[_cnt++] = BYTE2(d5);
    data2swji[_cnt++] = BYTE3(d5);

    data2swji[_cnt++] = BYTE0(d6);
    data2swji[_cnt++] = BYTE1(d6);
    data2swji[_cnt++] = BYTE2(d6);
    data2swji[_cnt++] = BYTE3(d6);

    data2swji[_cnt++] = BYTE0(d7);
    data2swji[_cnt++] = BYTE1(d7);
    data2swji[_cnt++] = BYTE2(d7);
    data2swji[_cnt++] = BYTE3(d7);

    data2swji[_cnt++] = BYTE0(d8);
    data2swji[_cnt++] = BYTE1(d8);
    data2swji[_cnt++] = BYTE2(d8);
    data2swji[_cnt++] = BYTE3(d8);

    data2swji[_cnt++] = BYTE0(d9);
    data2swji[_cnt++] = BYTE1(d9);
    data2swji[_cnt++] = BYTE2(d9);
    data2swji[_cnt++] = BYTE3(d9);

    data2swji[_cnt++] = BYTE0(d10);
    data2swji[_cnt++] = BYTE1(d10);
    data2swji[_cnt++] = BYTE2(d10);
    data2swji[_cnt++] = BYTE3(d10);


    for( i=0;i<data2swji[3]+4; i++)
    {
        sc += data2swji[i];
        ac += sc;
    }

    data2swji[_cnt++] = sc;
    data2swji[_cnt++] = ac;

    for(i=0;i<_cnt;i++)UART_send_char(data2swji[i]);
    //printf("%s\n",data2swji);

}
void USART1_niming_report_GPS(u8 fs,u8 sn,s32 lng,s32 lat,s32 ag,s16 ns,s16 es,s16 ds,u8 pd,u8 sa,u8 va)
{
  u8 _cnt=0;
    u8 sc = 0;
    u8 ac = 0;
    u8 i;
    data2swji[_cnt++] = 0xAA;
    data2swji[_cnt++] = 0xFF;
    data2swji[_cnt++] = 0x30;
    data2swji[_cnt++] = 23;

    data2swji[_cnt++] = BYTE0(fs);

    data2swji[_cnt++] = BYTE0(sn);

    data2swji[_cnt++] = BYTE0(lng);
    data2swji[_cnt++] = BYTE1(lng);
    data2swji[_cnt++] = BYTE2(lng);
    data2swji[_cnt++] = BYTE3(lng);

    data2swji[_cnt++] = BYTE0(lat);
    data2swji[_cnt++] = BYTE1(lat);
    data2swji[_cnt++] = BYTE2(lat);
    data2swji[_cnt++] = BYTE3(lat);

    data2swji[_cnt++] = BYTE0(ag);
    data2swji[_cnt++] = BYTE1(ag);
    data2swji[_cnt++] = BYTE2(ag);
    data2swji[_cnt++] = BYTE3(ag);

    data2swji[_cnt++] = BYTE0(ns);
    data2swji[_cnt++] = BYTE1(ns);

    data2swji[_cnt++] = BYTE0(ds);
    data2swji[_cnt++] = BYTE1(ds);

    data2swji[_cnt++] = BYTE0(es);
    data2swji[_cnt++] = BYTE1(es);

    data2swji[_cnt++] = BYTE0(pd);

    data2swji[_cnt++] = BYTE0(sa);

    data2swji[_cnt++] = BYTE0(va);

    for( i=0;i<data2swji[3]+4; i++)
    {
        sc += data2swji[i];
        ac += sc;
    }

    data2swji[_cnt++] = sc;
    data2swji[_cnt++] = ac;

    for(i=0;i<_cnt;i++)UART_send_char(data2swji[i]);
}
void USART1_niming_report_RC(s16 rol,s16 pit,s16 thr,s16 yaw,s16 aux1,s16 aux2,s16 aux3,s16 aux4,s16 aux5,s16 aux6)
{
  u8 _cnt=0;
    u8 sc = 0;
    u8 ac = 0;
    u8 i;
    data2swji[_cnt++] = 0xAA;
    data2swji[_cnt++] = 0xFF;
    data2swji[_cnt++] = 0x40;
    data2swji[_cnt++] = 20;

    data2swji[_cnt++] = BYTE0(rol);
    data2swji[_cnt++] = BYTE1(rol);

    data2swji[_cnt++] = BYTE0(pit);
    data2swji[_cnt++] = BYTE1(pit);

    data2swji[_cnt++] = BYTE0(thr);
    data2swji[_cnt++] = BYTE1(thr);

    data2swji[_cnt++] = BYTE0(yaw);
    data2swji[_cnt++] = BYTE1(yaw);

    data2swji[_cnt++] = BYTE0(aux1);
    data2swji[_cnt++] = BYTE1(aux1);

    data2swji[_cnt++] = BYTE0(aux2);
    data2swji[_cnt++] = BYTE1(aux2);

    data2swji[_cnt++] = BYTE0(aux3);
    data2swji[_cnt++] = BYTE1(aux3);

    data2swji[_cnt++] = BYTE0(aux4);
    data2swji[_cnt++] = BYTE1(aux4);

    data2swji[_cnt++] = BYTE0(aux5);
    data2swji[_cnt++] = BYTE1(aux5);

    data2swji[_cnt++] = BYTE0(aux6);
    data2swji[_cnt++] = BYTE1(aux6);

    for( i=0;i<data2swji[3]+4; i++)
    {
        sc += data2swji[i];
        ac += sc;
    }

    data2swji[_cnt++] = sc;
    data2swji[_cnt++] = ac;

    for(i=0;i<_cnt;i++)UART_send_char(data2swji[i]);
}
